#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<image_transport/image_transport.h>

#include<opencv2/opencv.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgcodecs/imgcodecs.hpp>
#include<opencv2/imgproc/imgproc.hpp>

#include<stdio.h>
#include<math.h>
#include<vector>
using namespace std;
using namespace cv;
cv::Mat m_color_image;

// 移除过小或过大的轮廓
void getSizeContours(vector<vector<Point>> &contours)
{
  //默认参数100，1000
  int cmin = 1000;   // 最小轮廓长度
	int cmax = 3800;   // 最大轮廓长度
	vector<vector<Point>>::const_iterator itc = contours.begin();
	while(itc != contours.end())
	{
		if((itc->size()) < cmin || (itc->size()) > cmax)
		{
			itc = contours.erase(itc);
		}
		else ++ itc;
	}
}


vector<Vec4f> findVertiLine(vector<Vec4f> &lines_std, float deta_tresh)
{
    std::vector<cv::Vec4f>::const_iterator itr = lines_std.begin();
    cv::Point pt1, pt2;
    vector<Vec4f> vlines;
    for (; itr != lines_std.end(); ++itr)
    {
        pt1.x = (*itr)[0];
	pt1.y = (*itr)[1];
	pt2.x = (*itr)[2];
	pt2.y = (*itr)[3];
	float line_angle = atan2(pt1.y - pt2.y, pt1.x - pt2.x);
	line_angle *= 57.19577951; // 180 / CV_PI
	//std::cout <<"line_angle = " <<line_angle << '\n';

	if (line_angle > (90-deta_tresh) && line_angle < (90+deta_tresh) ||
             line_angle > (-90-deta_tresh) && line_angle < (-90+deta_tresh))
	{
            std::cout<< "line_angle " << line_angle <<endl;

            vlines.push_back(*itr);
        }
     }
     return vlines;
}

cv::Mat dilateProcess(cv::Mat &cannyed) {
  //获取自定义核
    Mat element = getStructuringElement(MORPH_RECT, Size(10, 10));
    Mat out;
    //进行膨胀操作
    dilate(cannyed, out, element);
    cv::imshow("膨胀后", out);
    return out;
}
//得到联通区域
cv::Mat getImageContour(cv::Mat &src)
{
    vector<std::vector<cv::Point> >contours;
    findContours(src,contours,CV_RETR_CCOMP,CV_CHAIN_APPROX_NONE);
    Mat result(src.size(),CV_8U,Scalar(255));
    //移除过大或过小的轮廓元素
    getSizeContours(contours);
    drawContours(result,contours,-1,Scalar(0),2);
    imshow("Contours",result);
    return result;

}

void getImageLineLSD(cv::Mat &m_color_image)
{
  Ptr<LineSegmentDetector> ls = createLineSegmentDetector(LSD_REFINE_STD);
  vector<Vec4f> lines_std;
  ls->detect(m_color_image, lines_std);
  m_color_image = Scalar(0, 0, 255);
  //ls->drawSegments(m_color_image, lines_std);
  ls->drawSegments(m_color_image, findVertiLine(lines_std, 20.0f));   //lines_std为上步直线检测的结果

  imshow("video", m_color_image);
}

bool test(cv::Mat &src)
{
    cv::Mat dst;
    //src = imread("F:/picture/去除干扰线.png");
    if (src.empty()) {
        printf("could not image");
        return -1;
    }
    cv::Mat  gray_src;
    cv::cvtColor(src, gray_src, CV_BGR2GRAY);
    //cv::imshow("灰色", gray_src);

    //转换为二值图像
    cv::Mat binimage;
    cv::adaptiveThreshold(~gray_src, binimage, 255, ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 15, -2);
    cv::imshow("二值", binimage);
    cv::Mat vline = getStructuringElement(MORPH_RECT, Size(1,src.cols /32), Point(-1, - 1));
    cv::Mat yline = getStructuringElement(MORPH_RECT, Size(1, src.rows/16), Point(-1, -1));
    cv::Mat sline = getStructuringElement(MORPH_RECT, Size(3,3), Point(-1, -1));
    cv::Mat temp;
    cv::erode(binimage, temp, vline);
    cv::dilate(temp, dst, vline);
    //morphologyEx(binimage, dst, CV_MOP_OPEN, hline);
    //blur(dst, dst, Size(3, 3), Point(-1, -1));
    cv::bitwise_not(dst, dst);

    cv::imshow("vertical_line", dst);
    //cv::imshow("测试", src);
    cv::waitKey(0.5);
    //return 0;
}

//opencv官方参考
//https://docs.opencv.org/3.0.0/d8/dd4/lsd_lines_8cpp-example.html
void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
{

	ROS_INFO("Received \n");
        //cv::Mat m_color_image;
	try{
        m_color_image = cv_bridge::toCvShare(msg, "mono8")->image;  // single gray

        //m_color_image = cv_bridge::toCvShare(msg, "rgb8")->image;
        //cv::waitKey(30);
        }
    catch( cv_bridge::Exception& e )
    {
        ROS_ERROR( "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str() );
    }


}

void houghDetect(cv::Mat &m_color_image)
{
  if( m_color_image.empty() )
   {
     cout << "Unable to load Image" << endl;
     return ;
   }
  cv::imshow("原图像", m_color_image);
  cv::Mat canny;
  Canny(m_color_image, canny, 50, 250, 3);
  cv::Mat dst,vlinedst;
  cv::Mat dilated = dilateProcess(canny);
  cv::Mat contoured = getImageContour(dilated);
  cvtColor(contoured, dst, CV_GRAY2BGR);//将二值图转换为RGB图颜色空间，这里重新创建一张空Mat也行
  cvtColor(contoured, vlinedst, CV_GRAY2BGR);//将二值图转换为RGB图颜色空间，这里重新创建一张空Mat也行
  vector<Vec4f> plines;//保存霍夫变换检测到的直线
  getImageLineLSD(contoured);
  HoughLinesP(dilated, plines, 1, CV_PI / 180, 50, 100, 2);//提取边缘时，会造成有些点不连续，所以maxLineGap设大点
  //5. 显示检测到的直线
  Scalar color = Scalar(0, 255, 255);//设置颜色

  for (size_t i = 0; i < plines.size(); i++)
  {
    Vec4f hline = plines[i];
    line(dst, Point(hline[0], hline[1]), Point(hline[2], hline[3]), color, 2, LINE_AA);//绘制直线
  }
  imshow("plines", dst);
  auto plinesRefine = findVertiLine(plines, 10.0f);
  for (size_t i = 0; i < plinesRefine.size(); i++)
  {
    Vec4f hline = plinesRefine[i];
    line(vlinedst, Point(hline[0], hline[1]), Point(hline[2], hline[3]), color, 3, LINE_AA);//绘制直线
  }
  imshow("最终结果", vlinedst);

}


void lineProcess(cv::Mat &m_color_image)
{
        bool useRefine = false;  //是否平滑精细化处理
        bool useCanny = true; //不使用canny
        if( m_color_image.empty() )
         {
           cout << "Unable to load Image" << endl;
           return ;
         }
        cv::imshow("原图像", m_color_image);
        if (useCanny)
        Canny(m_color_image, m_color_image, 40, 200, 3);

        Ptr<LineSegmentDetector> ls = useRefine ? createLineSegmentDetector(LSD_REFINE_STD): createLineSegmentDetector(LSD_REFINE_NONE);

        vector<Vec4f> lines_std;

        ls->detect(m_color_image, lines_std);

         /*param
        2, lines：A vector of Vec4i or Vec4f elements specifying the beginning and ending point of a line. Where Vec4i/Vec4f is (x1, y1, x2, y2), point 1 is the start, point 2 - end. Returned lines are strictly oriented depending on the gradient.
       */
        // Show found lines

        if (useCanny)
        m_color_image = Scalar(0, 0, 255);
        //ls->drawSegments(m_color_image, lines_std);
        ls->drawSegments(m_color_image, findVertiLine(lines_std, 20.0f));   //lines_std为上步直线检测的结果

        imshow("video", m_color_image);

}
//腐蚀膨胀
/*
void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
{
        bool useRefine = false;  //是否平滑精细化处理
        bool useCanny = true; //不使用canny
	ROS_INFO("Received \n");
        cv::Mat m_color_image;

	//try{
        //cv::imshow( "video", cv_bridge::toCvShare(msg, "bgr8")->image );
        //m_color_image = cv_bridge::toCvShare(msg, "mono8")->image;  // single gray
        m_color_image = cv_bridge::toCvShare(msg, "rgb8")->image;
        //cv::waitKey(30);
        if( m_color_image.empty() )
         {
           cout << "Unable to load Image" << endl;
           return ;
         }
        test(m_color_image);
}
*/




int main(int argc, char** argv)
{


    ros::init(argc, argv, "image_listener");
    ros::NodeHandle n;
    ros::Rate loop_rate(10);

    image_transport::ImageTransport it(n);
    image_transport::Subscriber sub = it.subscribe( "/camera/color/image_raw", 1, imageCalllback );
    while (ros::ok())
    {
        ros::spinOnce();
        houghDetect(m_color_image);
        waitKey(2);
        loop_rate.sleep();
    }
    return 0;
}
